#include "lsp_server.h"
#include "lspmessage.pb-c.h"
#include "unistd.h"
#include <iostream>

#ifndef GLOBAL_EPOCH_VARS
#define GLOBAL_EPOCH_VARS
double epoch_lth = _EPOCH_LTH;
int epoch_cnt = _EPOCH_CNT;
double drop_rate = _DROP_RATE;
#endif

using namespace std;
/*
 *
 *
 *				LSP RELATED FUNCTIONS
 *
 *
 */  

void lsp_set_epoch_lth(double lth){epoch_lth = lth;}
void lsp_set_epoch_cnt(int cnt){epoch_cnt = cnt;}
void lsp_set_drop_rate(double rate){drop_rate = rate;}

/*
 *
 *
 *				SERVER RELATED FUNCTIONS
 *
 *
 */  


lsp_server* lsp_server_create(int port)
{
	lsp_server *new_lsp = new lsp_server();

	//Initialize data
	new_lsp->sockfd = -1;
	new_lsp->next_connid = 1;
	new_lsp->close_flag = false;
	new_lsp->read_lock = PTHREAD_MUTEX_INITIALIZER;
	new_lsp->to_send_lock = PTHREAD_MUTEX_INITIALIZER;
	new_lsp->epoch_lock = PTHREAD_MUTEX_INITIALIZER;
	new_lsp->new_msg_pushed = PTHREAD_COND_INITIALIZER;

	//Get socket file descriptor and save to fd
	if((new_lsp->sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
	{
		printf("Failed to get Socket FD while creating lsp server.\n");
		delete new_lsp; new_lsp = NULL;
		return NULL;
	}

	//Bind socket
	bzero(&(new_lsp->me),sizeof(new_lsp->me));
	new_lsp->me.sin_family = AF_INET;
	new_lsp->me.sin_addr.s_addr = INADDR_ANY;
	new_lsp->me.sin_port = htons(port);	
	
	if(bind(new_lsp->sockfd, (sockaddr*) &(new_lsp->me), sizeof(new_lsp->me)) < 0)
	{
		printf("Failed to bind socket while creating lsp server.\n");
	}

	//Start threads:
	pthread_create(&(new_lsp->epoch_thread), NULL, lsp_server_epoch_thread, (void*) new_lsp);
	pthread_create(&(new_lsp->lsp_thread), NULL, lsp_server_lsp_thread, (void*) new_lsp);

	return new_lsp;
}

int lsp_server_read(lsp_server* a_srv, void* pld, uint32_t* conn_id)
{
	//Wait for a message to be in the queue
	pthread_mutex_lock(&(a_srv->read_lock));
	while(a_srv->read.size() < 1)
	{
		pthread_cond_wait(&(a_srv->new_msg_pushed), &(a_srv->read_lock));
	}

	//Get lth and copy data to pld
	int lth = a_srv->read.front().payload.len;
	memcpy(pld, a_srv->read.front().payload.data, lth);
	
	//Copy connid
	*conn_id = a_srv->read.front().connid;

	//Pop the message and unlock
	delete a_srv->read.front().payload.data;
	a_srv->read.pop();
	pthread_mutex_unlock(&(a_srv->read_lock));

	return lth;
}

bool lsp_server_write(lsp_server* a_srv, void* pld, int lth, uint32_t conn_id)
{
	//Check if client is still connected
	if(a_srv->connection_lost[conn_id])
	{
		return false;
	}

	//Build LSPMessage
	LSPMessage msg = LSPMESSAGE__INIT;
	msg.connid = conn_id;
	msg.seqnum = 0;			//This will be changed before sending
	msg.payload.len = lth;
	msg.payload.data = (uint8_t*) malloc(lth);
	memcpy(msg.payload.data, pld, lth);

	//Add LSPMessage to queue to be sent
	pthread_mutex_lock(&(a_srv->to_send_lock));
	a_srv->to_send[conn_id].push(msg);
	pthread_mutex_unlock(&(a_srv->to_send_lock));

	return true;
}

bool lsp_server_close(lsp_server* a_srv, uint32_t conn_id)
{
	a_srv->close_flag = true;
	pthread_join(a_srv->epoch_thread, NULL);
	pthread_join(a_srv->lsp_thread, NULL);
	close(a_srv->sockfd);
	delete a_srv; a_srv = NULL;
	return true;
}

void *lsp_server_lsp_thread(void *a_srv)
{
	lsp_server* srv;
	srv = (lsp_server*) a_srv;

	while(!srv->close_flag)
	{
		//Setup Variables and recv UDP packet from socket
		uint8_t *buf;
		buf = (uint8_t*) malloc(1024);
		sockaddr from;
		socklen_t from_length = sizeof(from);
		int byte_count = recvfrom(srv->sockfd, buf, 1024, 0, (sockaddr*) &from, &from_length);
		//cout << "Msg recvd. byte_count = " << byte_count << endl;
		//Lock so epoch can't run while we are processing message
		pthread_mutex_lock(&(srv->epoch_lock));

		//Check to make sure packet was received
		if(byte_count > 0)
		{
			LSPMessage* msg;
			msg = lspmessage__unpack(NULL, byte_count, buf);   
	  		if (msg == NULL)
	    	{
	      		printf("Error unpacking incoming message\n");
	    	}
	    	else
	    	{
	    		int* lth;
				lth = (int*) &(msg->payload);
				if(msg->connid == 0 && msg->seqnum == 0)
				{
					//cout << "Msg is first from this client" << endl;
					//First message received from client
					//This is a connection request
					//Initialize the connection
					
					//Initialze connection variables
					srv->seqnum[srv->next_connid] = 1;
					srv->client[srv->next_connid];
					memset(&(srv->client[srv->next_connid]), 0, sizeof(sockaddr_in));
					memcpy(&(srv->client[srv->next_connid]), &from, from_length);
					srv->last_sent[srv->next_connid] = LSPMESSAGE__INIT;
					srv->epochs_since_contact[srv->next_connid] = 0;
					srv->waiting_on_ack[srv->next_connid] = false;
					srv->connection_lost[srv->next_connid] = false;
					srv->to_send[srv->next_connid];

					//Build ack message
					LSPMessage* last_sent;
					last_sent = &(srv->last_sent[srv->next_connid]);
					last_sent->seqnum = 0;
					last_sent->connid = srv->next_connid;
					last_sent->payload.len = 0;
					last_sent->payload.data = NULL;

					//Increment next_connid
					srv->next_connid++;

					//Send ack
					lsp_server_send_last(srv, srv->next_connid - 1);

				}
				else if(msg->connid >= srv->next_connid)
				{
					printf("Recvd message non-valid connid\n");
				}
				else if(msg->seqnum < srv->seqnum[msg->connid])
				{
					//cout << "Msg is old" << endl;
					//Ignore message. The seqnum is old. We have already received it once.
					srv->epochs_since_contact[msg->connid] = 0;
				}
				else if(*lth <= 0 && msg->seqnum == srv->seqnum[msg->connid])
				{
					//cout << "Msg is an ack" << endl;
					//Deal with ack here
					srv->seqnum[msg->connid]++;
					srv->waiting_on_ack[msg->connid] = false;
					srv->epochs_since_contact[msg->connid] = 0;
				}
				else if(*lth > 0 && msg->seqnum >= srv->seqnum[msg->connid])
				{
					//cout << "Msg has pld: " << string((char*) msg->payload.data) << endl;
					//Deal with payload message here
					//Add to read queue and signal other thread
					pthread_mutex_lock(&(srv->read_lock));
					srv->read.push(*msg);
					pthread_mutex_unlock(&(srv->read_lock));
					pthread_cond_signal(&(srv->new_msg_pushed));

					//Build ack message
					srv->last_sent[msg->connid].seqnum = msg->seqnum;
					srv->last_sent[msg->connid].connid = msg->connid;
					srv->last_sent[msg->connid].payload.len = 0;
					delete srv->last_sent[msg->connid].payload.data;
					srv->last_sent[msg->connid].payload.data = NULL;

					//Send ack message
					lsp_server_send_last(srv,msg->connid);

					srv->seqnum[msg->connid]++;
					srv->epochs_since_contact[msg->connid] = 0;
				}
	    	}
	    	delete msg; msg = NULL;
		}

		//Check to_send queue and send message if possible
		std::map< int,std::queue<LSPMessage> >::iterator it = srv->to_send.begin();
		for(; it != srv->to_send.end(); ++it)
		{
			//If there is a message to send and not waiting on ack
			if(it->second.size() > 0 && !(srv->waiting_on_ack[it->first]))
			{
				//Lock to_send lock
				pthread_mutex_lock(&(srv->to_send_lock));
				
				//Setup pointers to the message to be sent and last_sent
				LSPMessage* front;
				LSPMessage* last_sent;
				front = &(srv->to_send[it->first].front());
				last_sent = &(srv->last_sent[it->first]);

				//Set seqnum and connid
				last_sent->seqnum = srv->seqnum[it->first];
				last_sent->connid = it->first;
				
				//Set payload length
				last_sent->payload.len = front->payload.len;

				//Delete old payload data and copy new stuff over
				delete last_sent->payload.data;
				last_sent->payload.data = (uint8_t*) malloc(front->payload.len);
				memcpy(last_sent->payload.data, front->payload.data, front->payload.len);

				//Delete data before poping from queue
				delete front->payload.data;
				srv->to_send[it->first].pop();

				//Unlock to_send lock
				pthread_mutex_unlock(&(srv->to_send_lock));

				srv->waiting_on_ack[it->first] = true;
				lsp_server_send_last(srv, it->first);
			}
		}

		//Unlock so epoch can run
		pthread_mutex_unlock(&(srv->epoch_lock));

		//Clean up memory
		delete buf; buf = NULL;
	}

	return NULL;
}

void *lsp_server_epoch_thread(void *a_srv)
{
	lsp_server* srv;
	srv = (lsp_server*) a_srv;

	timeval last;
	timeval now;
	double time_left = epoch_lth;
	
	while(!srv->close_flag)
	{
		//Loop and sleep until the epoch has passed
		gettimeofday(&last, NULL);
		time_left = epoch_lth;
		while(time_left > 0)
		{
			usleep((int) (time_left * 1000000));
			gettimeofday(&now, NULL);
			time_left = epoch_lth - 
				((double) now.tv_sec + ((double) now.tv_usec/1000000)
					- ((double) last.tv_sec + ((double) last.tv_usec/1000000)));
		}

		//Time to run epoch stuff. Get lock.
		pthread_mutex_lock(&(srv->epoch_lock));

		//Increment epochs_since_contact and send last if still connected
		std::map<int,int>::iterator it2 = srv->epochs_since_contact.begin();
		for(; it2 != srv->epochs_since_contact.end(); ++it2)
		{
			it2->second++;

			//If its been to many, set as connection lost
			if(it2->second > epoch_cnt)
			{
				srv->connection_lost[it2->first] = true;
			}
			//else resend the last message
			else
			{
				lsp_server_send_last(srv, it2->first);
			}
		}

		//Done with epoch. Release epoch lock.
		pthread_mutex_unlock(&(srv->epoch_lock));
	}
	return NULL;
}

void lsp_server_send_last(lsp_server* srv, int connid)
{
	//Gen random number [0,1] for packet dropping
	double x = ((double) rand()/ (RAND_MAX));
	if(x > drop_rate)
	{
		LSPMessage* last_sent;
		last_sent = &(srv->last_sent[connid]);
		void* buf;
		size_t len = lspmessage__get_packed_size(last_sent);
		buf = malloc(len);
		lspmessage__pack(last_sent, (uint8_t*) buf);
		int client_size = sizeof(srv->client[connid]);
		sendto(srv->sockfd, buf, len, 0, (sockaddr*) &(srv->client[connid]), client_size);
		/*cout << "Sending last now. Seqnum: " << last_sent->seqnum 
				<< "  connid: " << connid
				<< "  len: " << last_sent->payload.len << endl;*/
		delete buf;
	}
}

